/*
 * =====================================================================================
 *
 *       Filename:  point_detection.h
 *
 *    Description:  point detection
 *
 *        Version:  1.0
 *        Created:  2009年06月08日 18时58分52秒
 *       Revision:  none
 *       Compiler:  gcc
 *
 *         Author:  Ying Wang (WY), ywang@nlpr.ia.ac.cn
 *        Company:  Institute of Automation, Chinese Academy of Sciences
 *
 * =====================================================================================
 */

#ifndef POINT_DETECTION_H
#define POINT_DETECTION_H
#include "point.h"
#include "../process.h"
#include <vector>
typedef std::vector<NCpoint<int> > NCpointVector;

/**
 * |rx2  rxry|
 * |rxry ry2 |
 * step:
 * 1. apply corner operator
 * 2. threshold cornerness map
 * 3. Non-maximal suppression
 */
NCpointVector harris_corner (const NCmatrix<double> &mat, const NCmatrix<double> &kernel,double thr=200,double k=0.04)
{
	int i,j,hi,hj,row=mat.row(), column=mat.column(),hrow=kernel.row(),hcolumn=kernel.column();
	double w,rx,ry,rx2,ry2,rxry,tmp;
	NCmatrix<double> cornernessmap(row,column),gradx = gradientx(mat), grady = gradienty(mat);
	NCpointVector pointVec;
	std::vector<double> values;
	for( i=0; i<row; i++ )
	{
		for( j=0; j<column; j++ )
		{
			rx2=0;ry2=0;rxry=0;
			for( hi=0; hi<hrow; hi++)
			{
				for ( hj=0; hj<hcolumn; hj++ )
				{
					if ( 
							(i-(hrow-1)/2+hi)>0    && 
							(j-(hcolumn-1)/2+hj)>0 && 
							(i-(hrow-1)/2+hi)<row    && 
							(j-hcolumn/2+hj)<column 
					   )
					{
						w = kernel[hi][hj];
						rx = gradx[i-(hrow-1)/2+hi][j-(hcolumn-1)/2+hj];
						ry = grady[i-(hrow-1)/2+hi][j-(hcolumn-1)/2+hj];
						rx2 += w*rx*rx;
						ry2 += w*ry*ry;
						rxry += w*rx*ry;
					}
				}
			}
			tmp = (rx2*ry2-rxry*rxry-k*(rx2+ry2)*(rx2+ry2)) - thr ;
			if ( tmp>0 )
			{
				cornernessmap[i][j] = tmp;
			}
			else
			{
				cornernessmap[i][j] = 0.;
			}
			/*if( tmp> 0 )
			{
				pointVec.push_back(NCpoint<int>(i,j));
				values.push_back(tmp);
			}*/

		}
	}
	int r=7;
	bool bmax;
	for( i=1; i<row-1; i++ )
	{
		for( j=1; j<column-1; j++ )
		{
			tmp = cornernessmap[i][j];
			bmax = true;
			/*if(  
					(tmp>cornernessmap[i-1][j-1])
			  	     && (tmp>cornernessmap[i-1][j])
				     && (tmp>cornernessmap[i-1][j+1])
				     && (tmp>cornernessmap[i][j-1])
			  	     && (tmp>cornernessmap[i][j+1])
			  	     && (tmp>cornernessmap[i+1][j-1])
			  	     && (tmp>cornernessmap[i+1][j])
				     && (tmp>cornernessmap[i+1][j+1])
			  )*/
			for( hi=-r;hi<=r;hi++ )
			{
				for( hj=-r; hj<=r; hj++ )
				{
					if( (i+hi)>=0 && (i+hi)<row && (j+hj)>=0 && (j+hj)<column )
					{
						if( tmp<cornernessmap[i+hi][j+hj] )
							bmax = false;
					}
				}
			}
			if(bmax)
			{
				pointVec.push_back(NCpoint<int>(i,j));
				values.push_back(cornernessmap[i][j]);
			}

		}
	}
	//sort by descend bubble sort
	NCpoint<int> tmpp;
	int N = values.size();
	bool exchange;
	double tmpv;
	for( i=1; i<N; i++ )
	{
		exchange = false;
		for( j=N-1; j>=1; j-- )
		{
			if(values[j]>values[j-1])
			{
				tmpv = values[j-1];
				tmpp = pointVec[j-1];
				values[j-1] = values[j];
				pointVec[j-1] = pointVec[j];
				values[j] = tmpv;
				pointVec[j] = tmpp;
				exchange = true;
			}
		}
		if(!exchange)
			break;
	}

	return pointVec;
}
#endif
